#ifndef InteractionCalculator_MohdYusof_H_
#define InteractionCalculator_MohdYusof_H_

#include <NJRLib/Interfaces/vector3d.h>
#include <vector>
#include <set>
#include <assert.h>
#include "olb3D.h"
#include <ibm/ibm3D.h>

#include <iostream>
using namespace std;

using namespace olb;

#include <math.h>
class InteractionCalculator_MohdYusof
{
public:
	InteractionCalculator_MohdYusof() {}
	
	template<typename T, template<typename U> class Lattice>
	void CalculateInteraction(const BlockStructure3D<T, Lattice>& lattice, T rho);

	template<typename T, template<typename U> class Lattice>
	void ParallelCalculateInteraction(const MultiBlockLattice3D<T, Lattice>& lattice, T rho);

	template<typename T, template<typename U> class Lattice>
	void DistributeFluidBodyForce(BlockStructure3D<T, Lattice>& lattice, double extend);

	template<typename T, template<typename U> class Lattice>
	void ParallelDistributeFluidBodyForce(MultiBlockLattice3D<T, Lattice>& lattice, double extend);
								
	void ClearAll() { this->intPts.clear(); this->lookupID.clear(); this->linearForce.clear(); this->torque.clear(); this->p_center.clear(); this->p_vel.clear(); }
	void AddDO(NJRvector3d particleCenter, NJRvector3d particleVelocity, std::vector<IntegrationPoint*>& integrationPoints);
	int GetNumberOfDO() const { return this->p_center.size(); }
	const NJRvector3d& GetForces(int i) const { return this->linearForce[i]; }
	const NJRvector3d& GetTorque(int i) const { return this->torque[i]; }
	
	double DeltaFunction(double r) const { return fabs(r) <= 2. ? 0.25 * (1.0 + cos(0.5*M_PI*fabs(r))) : 0.0; }
	
private:
	std::vector<IntegrationPoint*> intPts;
	std::vector<int> lookupID;
	//for DO
	std::vector<NJRvector3d> linearForce;
	std::vector<NJRvector3d> torque;
	std::vector<NJRvector3d> p_center;
	std::vector<NJRvector3d> p_vel;
	void CalDOForceAndTorque();
};

void InteractionCalculator_MohdYusof::AddDO(NJRvector3d particleCenter, NJRvector3d particleVelocity, std::vector<IntegrationPoint*>& integrationPoints)
{
	int DOid = this->p_center.size();
	this->p_center.push_back(particleCenter);
	this->p_vel.push_back(particleVelocity);
	for(int i=0; i<integrationPoints.size(); i++)
	{
		this->lookupID.push_back(DOid);
		IntegrationPoint* pt = integrationPoints[i];
		this->intPts.push_back(pt);
	}
}

void InteractionCalculator_MohdYusof::CalDOForceAndTorque()
{
	this->linearForce.clear();
	this->torque.clear();
	for(unsigned int i = 0; i < p_center.size(); i++)
	{
		this->linearForce.push_back(NJRvector3d(0.0, 0.0, 0.0));
		this->torque.push_back(NJRvector3d(0.0, 0.0, 0.0));
	}
		
	for(unsigned int i = 0; i < this->intPts.size(); i++)
	{
		int doID = this->lookupID[i];
		NJRvector3d linearF = this->intPts[i]->GetCurrentInteractionForce();
		NJRvector3d r = NJRvector3d(intPts[i]->x, intPts[i]->y, intPts[i]->z) - this->p_center[doID];
		this->linearForce[doID] += linearF;
		this->torque[doID] += r.Cross(linearF);
	}
}

#ifdef PARALLEL_MODE_MPI

template<typename T, template<typename U> class Lattice>
void InteractionCalculator_MohdYusof::ParallelCalculateInteraction(const MultiBlockLattice3D<T, Lattice>& lattice, T rho)
{
	//1. dispatch integration potins
	vector<double> localPt_x, localPt_y, localPt_z;
	int iBlock = singleton::mpi().getRank();
	BlockParameters3D const& blockParam = lattice.getMultiData().getBlockParameters(iBlock);
	BlockCoordinates3D const& bulk = blockParam.getBulk();
	for(unsigned int i = 0; i < this->intPts.size(); i++)
	{
		IntegrationPoint &pt = *(this->intPts[i]);
		localPt_x.push_back(blockParam.toLocalX(pt.x));
		localPt_y.push_back(blockParam.toLocalY(pt.y));
		localPt_z.push_back(blockParam.toLocalZ(pt.z));
	}
	
	//2. calculate local forces
	vector<T> local_forces(p_center.size()*3);
	vector<T> local_torque(p_center.size()*3);	
	BlockLattice3D<T,Lattice>* localLattice = lattice.getBlockLattices()[iBlock];
	for(unsigned int i = 0; i < this->intPts.size(); i++)
	{
		IntegrationPoint &pt = *(this->intPts[i]);

		int id_nx = (int)floor(localPt_x[i]);
		int id_ny = (int)floor(localPt_y[i]);
		int id_nz = (int)floor(localPt_z[i]);	
		
		int id_px = id_nx + 1;
		int id_py = id_ny + 1;
		int id_pz = id_nz + 1;		
		
		T w_px = localPt_x[i] - (double)(id_nx); 
		T w_py = localPt_y[i] - (double)(id_ny);
		T w_pz = localPt_z[i] - (double)(id_nz);
		
		T w_nx = 1.0 - w_px;
		T w_ny = 1.0 - w_py;
		T w_nz = 1.0 - w_pz;

		T u[3] = { T(), T(), T() };
		T tmpU[3];
		localLattice->get(id_nx, id_ny, id_nz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim]  = tmpU[dim] * w_nx * w_ny * w_nz;
		localLattice->get(id_nx, id_ny, id_pz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_nx * w_ny * w_pz;
		localLattice->get(id_nx, id_py, id_nz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_nx * w_py * w_nz;
		localLattice->get(id_nx, id_py, id_pz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_nx * w_py * w_pz;
		localLattice->get(id_px, id_ny, id_nz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_px * w_ny * w_nz;
		localLattice->get(id_px, id_ny, id_pz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_px * w_ny * w_pz;
		localLattice->get(id_px, id_py, id_nz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_px * w_py * w_nz;
		localLattice->get(id_px, id_py, id_pz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_px * w_py * w_pz;
		
		T g[3];
		int doID = this->lookupID[i];
		g[0] = this->p_vel[doID].x() - u[0];
		g[1] = this->p_vel[doID].y() - u[1];
		g[2] = this->p_vel[doID].z() - u[2];
		
		double intVol = pt.GetIntegrationVolume();
		double fx	  = -g[0] * intVol;
		double fy 	  = -g[1] * intVol;
		double fz	  = -g[2] * intVol;
		
		pt.SetCurrentInteractionForce(NJRvector3d(fx, fy, fz));
		
		NJRvector3d r = NJRvector3d(intPts[i]->x, intPts[i]->y, intPts[i]->z) - this->p_center[doID];
		NJRvector3d torque_vec = r.Cross(NJRvector3d(fx, fy, fz));
		local_forces[doID*3]   += fx;
		local_forces[doID*3+1] += fy;
		local_forces[doID*3+2] += fz;
		local_torque[doID*3]   += torque_vec.x();
		local_torque[doID*3+1] += torque_vec.y();
		local_torque[doID*3+2] += torque_vec.z();
	}
	
	//3. sum global forces
	vector<T> sum_forces(p_center.size()*3);
	vector<T> sum_torque(p_center.size()*3);
	singleton::mpi().allReduceVect(local_forces, sum_forces, MPI_SUM);
	singleton::mpi().allReduceVect(local_torque, sum_torque, MPI_SUM);
		
	//4. calculate linear force and torque
	this->linearForce.clear();
	this->torque.clear();
	for(unsigned int i = 0; i < p_center.size(); i++)
	{
		this->linearForce.push_back(NJRvector3d(sum_forces[i*3], sum_forces[i*3+1], sum_forces[i*3+2]));
		this->torque.push_back(NJRvector3d(sum_torque[i*3], sum_torque[i*3+1], sum_torque[i*3+2]));
	}
}
#endif

template<typename T, template<typename U> class Lattice>
void InteractionCalculator_MohdYusof::CalculateInteraction(const BlockStructure3D<T, Lattice>& lattice, T rho)
{
	for(unsigned int i = 0; i < this->intPts.size(); i++)
	{
		IntegrationPoint &pt = *(this->intPts[i]);

		int id_nx = (int)floor(pt.x);
		int id_ny = (int)floor(pt.y);
		int id_nz = (int)floor(pt.z);

		int id_px = id_nx + 1;
		int id_py = id_ny + 1;
		int id_pz = id_nz + 1;
		
		T w_px = pt.x - (double)(id_nx); 
		T w_py = pt.y - (double)(id_ny);
		T w_pz = pt.z - (double)(id_nz);
		
		T w_nx = 1.0 - w_px;
		T w_ny = 1.0 - w_py;
		T w_nz = 1.0 - w_pz;

		T u[3];
		T tmpU[3];

		lattice.get(id_nx, id_ny, id_nz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim]  = tmpU[dim] * w_nx * w_ny * w_nz;
		lattice.get(id_nx, id_ny, id_pz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_nx * w_ny * w_pz;
		lattice.get(id_nx, id_py, id_nz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_nx * w_py * w_nz;
		lattice.get(id_nx, id_py, id_pz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_nx * w_py * w_pz;
		lattice.get(id_px, id_ny, id_nz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_px * w_ny * w_nz;
		lattice.get(id_px, id_ny, id_pz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_px * w_ny * w_pz;
		lattice.get(id_px, id_py, id_nz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_px * w_py * w_nz;
		lattice.get(id_px, id_py, id_pz).computeU(tmpU);
		for(int dim = 0; dim < 3; dim++) u[dim] += tmpU[dim] * w_px * w_py * w_pz;
		
		T g[3];
		int doID = this->lookupID[i];
		g[0] = this->p_vel[doID].x() - u[0];
		g[1] = this->p_vel[doID].y() - u[1];
		g[2] = this->p_vel[doID].z() - u[2];
		
		double intVol = pt.GetIntegrationVolume();
		double forceX = -g[0] * intVol;
		double forceY = -g[1] * intVol;
		double forceZ = -g[2] * intVol;
		
		pt.SetCurrentInteractionForce(NJRvector3d(forceX, forceY, forceZ));
	}

	// 2. calculate linear forces and torques
	this->CalDOForceAndTorque();
}

#include <assert.h>

		
#ifdef PARALLEL_MODE_MPI
template<typename T, template<typename U> class Lattice>
void InteractionCalculator_MohdYusof::ParallelDistributeFluidBodyForce(MultiBlockLattice3D<T, Lattice>& lattice, double extend)
{
	int numBlock = singleton::mpi().getSize();
	int iBlock = singleton::mpi().getRank();
	BlockLattice3D<T,Lattice>* localLattice = lattice.getBlockLattices()[iBlock];
	
	vector<int> num_pt_TO_other_blocks(numBlock);
	vector< vector<double> > pt_data_TO_other_blocks(numBlock);

	for(unsigned int i = 0; i < this->intPts.size(); i++)
	{
		IntegrationPoint &pt = *(intPts[i]);
		
		int minID_x = (int)floor(pt.x-extend) - 1;
		int minID_y = (int)floor(pt.y-extend) - 1;
		int minID_z = (int)floor(pt.z-extend) - 1;
		int maxID_x = (int)floor(pt.x+extend) + 1;
		int maxID_y = (int)floor(pt.y+extend) + 1;
		int maxID_z = (int)floor(pt.z+extend) + 1;
	
		for(int blockID = 0; blockID < numBlock; blockID++)
		{
			BlockParameters3D const& blockParam = lattice.getMultiData().getBlockParameters(blockID);
			BlockCoordinates3D domain(minID_x, maxID_x, minID_y, maxID_y, minID_z, maxID_z), inters;
			
			if(blockID == iBlock)
			{
				if (util::intersect(domain, blockParam.getEnvelope(), inters ) ) {
					for(int nx = inters.x0; nx <= inters.x1; nx++)
						for(int ny = inters.y0; ny <= inters.y1; ny++)
							for(int nz = inters.z0; nz <= inters.z1; nz++)
							{		
							T* force = localLattice->get(blockParam.toLocalX(nx), blockParam.toLocalY(ny), blockParam.toLocalZ(nz)).getExternal(Lattice<T>::ExternalField::forceBeginsAt);
							double dx = (double)nx - pt.x;
							double dy = (double)ny - pt.y;
							double dz = (double)nz - pt.z;			
							double weight = DeltaFunction(dx) * DeltaFunction(dy) * DeltaFunction(dz) / pt.GetIntegrationLength();
							force[0] += -1.0 * pt.GetCurrentInteractionForce().x() * weight;							
							force[1] += -1.0 * pt.GetCurrentInteractionForce().y() * weight;
							force[2] += -1.0 * pt.GetCurrentInteractionForce().z() * weight;	
							}
				}
			}
			else
			{
				if (util::intersect(domain, blockParam.getEnvelope(), inters ) )
				{
					num_pt_TO_other_blocks[blockID]++;
					pt_data_TO_other_blocks[blockID].push_back(pt.x);
					pt_data_TO_other_blocks[blockID].push_back(pt.y);
					pt_data_TO_other_blocks[blockID].push_back(pt.z);
					pt_data_TO_other_blocks[blockID].push_back(pt.GetCurrentInteractionForce().x());
					pt_data_TO_other_blocks[blockID].push_back(pt.GetCurrentInteractionForce().y());
					pt_data_TO_other_blocks[blockID].push_back(pt.GetCurrentInteractionForce().z());
					pt_data_TO_other_blocks[blockID].push_back(pt.GetIntegrationLength());
				}
			}
		}
	}

	vector<int> num_pt_FROM_other_blocks(numBlock);
	MPI_Alltoall( &num_pt_TO_other_blocks[0], 1, MPI_INT, &num_pt_FROM_other_blocks[0], 1, MPI_INT, MPI_COMM_WORLD);
	
	vector< vector<double> > pt_data_FROM_other_blocks(numBlock);
	for(int blockID = 0; blockID < numBlock; blockID++)
		if(blockID != iBlock)
			pt_data_FROM_other_blocks[blockID].resize(num_pt_FROM_other_blocks[blockID] * 7);

	for(int blockID = 0; blockID < numBlock; blockID++)
	{
		if(blockID == iBlock)
		{
			for(int to_blockID = 0; to_blockID < numBlock; to_blockID++)
				if(to_blockID != iBlock && pt_data_TO_other_blocks[to_blockID].size() > 0)
					singleton::mpi().send<double>(&(pt_data_TO_other_blocks[to_blockID][0]), pt_data_TO_other_blocks[to_blockID].size(), to_blockID);
		}
		else
			if(pt_data_FROM_other_blocks[blockID].size() > 0)
				singleton::mpi().receive<double>(&(pt_data_FROM_other_blocks[blockID][0]), pt_data_FROM_other_blocks[blockID].size(), blockID);
	}

	for(int blockID = 0; blockID < numBlock; blockID++)
	{
		int ptNum = num_pt_FROM_other_blocks[blockID];
		for(unsigned int i = 0; i < ptNum; i++)
		{
			double pt_x 	 = pt_data_FROM_other_blocks[blockID][i*7];
			double pt_y 	 = pt_data_FROM_other_blocks[blockID][i*7+1];
			double pt_z 	 = pt_data_FROM_other_blocks[blockID][i*7+2];
			double pt_Fx 	 = pt_data_FROM_other_blocks[blockID][i*7+3];
			double pt_Fy 	 = pt_data_FROM_other_blocks[blockID][i*7+4];
			double pt_Fz 	 = pt_data_FROM_other_blocks[blockID][i*7+5];
			double pt_IntLen = pt_data_FROM_other_blocks[blockID][i*7+6];

			int minID_x = (int)floor(pt_x-extend) - 1;
			int minID_y = (int)floor(pt_y-extend) - 1;
			int minID_z = (int)floor(pt_z-extend) - 1;
			int maxID_x = (int)floor(pt_x+extend) + 1;
			int maxID_y = (int)floor(pt_y+extend) + 1;
			int maxID_z = (int)floor(pt_z+extend) + 1;
		
			BlockParameters3D const& blockParam = lattice.getMultiData().getBlockParameters(iBlock);
			BlockCoordinates3D domain(minID_x, maxID_x, minID_y, maxID_y, minID_z, maxID_z), inters;
			if (util::intersect(domain, blockParam.getEnvelope(), inters ) ) {
				for(int nx = inters.x0; nx <= inters.x1; nx++)
					for(int ny = inters.y0; ny <= inters.y1; ny++)
						for(int nz = inters.z0; nz <= inters.z1; nz++)
						{		
						T* force = localLattice->get(blockParam.toLocalX(nx), blockParam.toLocalY(ny), blockParam.toLocalZ(nz)).getExternal(Lattice<T>::ExternalField::forceBeginsAt);
						double dx = (double)nx - pt_x;
						double dy = (double)ny - pt_y;
						double dz = (double)nz - pt_z;			
						double weight = DeltaFunction(dx) * DeltaFunction(dy) * DeltaFunction(dz) / pt_IntLen;
						force[0] += -1.0 * pt_Fx * weight;
						force[1] += -1.0 * pt_Fy * weight;
						force[2] += -1.0 * pt_Fz * weight;
						}
			}
		}
	}

}
#endif

template<typename T, template<typename U> class Lattice>
void InteractionCalculator_MohdYusof::DistributeFluidBodyForce(BlockStructure3D<T, Lattice>& lattice, double extend)
{
	for(unsigned int i = 0; i < this->intPts.size(); i++)
	{
		IntegrationPoint &pt = *(this->intPts[i]);
		
		int minID_x = (int)floor(pt.x-extend);
		int minID_y = (int)floor(pt.y-extend);
		int minID_z = (int)floor(pt.z-extend);
		int maxID_x = (int)floor(pt.x+extend) + 1;
		int maxID_y = (int)floor(pt.y+extend) + 1;
		int maxID_z = (int)floor(pt.z+extend) + 1;

		for(int nx = minID_x; nx <= maxID_x; nx++)
			for(int ny = minID_y; ny <= maxID_y; ny++)
				for(int nz = minID_z; nz <= maxID_z; nz++)
				{
					T* force = lattice.get(nx,ny,nz).getExternal(Lattice<T>::ExternalField::forceBeginsAt);
					double dx = (double)nx - pt.x;
					double dy = (double)ny - pt.y;
					double dz = (double)nz - pt.z;			
					double weight = DeltaFunction(dx) * DeltaFunction(dy) * DeltaFunction(dz) / pt.GetIntegrationLength();
					force[0] += -1.0 * pt.GetCurrentInteractionForce().x() * weight;							
					force[1] += -1.0 * pt.GetCurrentInteractionForce().y() * weight;
					force[2] += -1.0 * pt.GetCurrentInteractionForce().z() * weight;	
				}
	}
}

#endif /*InteractionCalculator_MohdYusof_H_*/
